function sk6config = SK6_Config( sCFG )
% SHAKE SK6 Matlab Driver v1.00, (c) 2010 SAMH Engineering Services
%
% function sk6config = SK6_Config( sCFG )
% Converts human readable configuration parameters into
% a cell array of address and data strings that can be used
% to update the configuration registers of the SK6.
% If no argument structure is provided then the values below are used.
% Modify the values below and provide no arguement when calling this
% function as an alternative way of configuring.
% Parameters that are not listed are not yet supported by this driver.
%
% Here is a full list of the parameters that are currently supported -
% sCFG.Receive_Parse_Rate            =  the number of times per second that the data received from the SK6 is parsed. Higher values decrease latency at the expensive of processing load.
% sCFG.Hardware_Input_Buff_Size      =  the size of the serial port recieve buffer - higher values allow longer periods of processing before more data needs to be parsed
% sCFG.Acceleration_Output_Data_Rate =  the acceleration output data rate in units of Hz
% sCFG.Gyro_Output_Data_Rate         =  the angular rate (gyro) output data rate in units of Hz
% sCFG.Magnetometer_Output_Data_Rate =  the magnetometer output data rate in units of Hz
% sCFG.Quaternion_Output_Data_Rate   =  the Atttitude and Heading in Quaternion format output data rate in units of Hz
% sCFG.Gyro_Temperature_Output       =  set to true to enable gyro temperature packets ( at a rate of 2Hz ), set to false to disable
% sCFG.Navigation_Switch_Output      =  set to true to enable navigation switch output packets, set to false to disable
% sCFG.Acceleration_Digital_Filter   =  set the acceleration digital filter to either minimum phase, 'MP', linear phase 'LP' or off 'OFF'
% sCFG.Gyro_Digital_Filter           =  set the gyro digital filter to either minimum phase, 'MP', linear phase 'LP' or off 'OFF'
% sCFG.Magnetometer_Digital_Filter   =  set the magnetometer digital filter to either minimum phase, 'MP', linear phase 'LP' or off 'OFF'
% sCFG.Acceleration_Range            =  set the range of the accelerometers to either '2g' for +/-2g or '6g' for +/-6g
% sCFG.Gyro_Range                    =  set the gyro range to either '300deg' for +/-300deg/s or '900deg' for +/-900 deg/s
% sCFG.Acceleration_Calibration      =  set which calibration coefficients to use, 'FACTORY' for the factory set, 'USER' for the user set or 'OFF' to disable calibration
% sCFG.Gyro_Calibration              =  set which calibration coefficients to use, 'FACTORY' for the factory set, 'USER' for the user set or 'OFF' to disable calibration
% sCFG.Magnetometer_Calibration      =  set which calibration coefficients to use, 'FACTORY' for the factory set, 'USER' for the user set or 'OFF' to disable calibration
% sCFG.Accelerometer_Cross_Axis			 =  set to 'ON' to enable accelerometer cross axis error compensation				
% sCFG.Gyro_Cross_Axis	        		 =  set to 'ON' to enable gyro cross axis error compensation
% sCFG.Magnetometer_Cross_Axis			 =  set to 'ON' to enable magnetometer cross axis error compensation
% sCFG.Gyro_Specific_Force					 =  set to 'ON' to enable gyro specific force error compensation        			
% sCFG.RPH_Algorithm								 =  set the attitude calculation algorithm to 'Legacy' or 'Fusion' where the latter uses the gyros in addition to accelerometer and magnetometer
% sCFG.RPH_Output_Format						 =  set the data format to 'Quaternion' or 'Euler'
% sCFG.RPH_Azimuth_Reference				 =  set the azimuth reference to 'Magnetometer' or 'Disabled' where the latter provides no feedback for the azimuth angle, it is just the integrated gyro rate.
% sCFG.RPH_Azimuth_Reset						 =  set to 'Enabled' to allow the use of the navigation switch to reset the azimuth angle to zero when the magnetometer is not used for feedback
%
% This function can be called at any time to retrieve the configuration values
% that were used by SK6_Start() when configuring the SK6 on first connection

if nargin == 0
  sCFG.Receive_Parse_Rate						  = 20;		% the number of times per second that the data received from the SK6 is parsed. Higher values decrease latency at the expensive of processing load.
  sCFG.Hardware_Input_Buff_Size 			= 16384; % sCFG.Hardware_Input_Buff_Size sets the size of the serial port recieve buffer - higher values allow longer periods of processing before more data needs to be parsed
  sCFG.Acceleration_Output_Data_Rate  = 10;  % the acceleration output data rate in units of Hz
  sCFG.Gyro_Output_Data_Rate 				  = 10;  % the angular rate (gyro) output data rate in units of Hz
  sCFG.Magnetometer_Output_Data_Rate  = 10;  % the magnetometer output data rate in units of Hz
  sCFG.Quaternion_Output_Data_Rate 	  = 000;	% the Atttitude and Heading in Quaternion format output data rate in units of Hz
  sCFG.Gyro_Temperature_Output 			  = false; % set to true to enable gyro temperature packets ( at a rate of 2Hz ), set to false to disable
  sCFG.Navigation_Switch_Output		    = true; % set to true to enable navigation switch output packets, set to false to disable
  sCFG.Acceleration_Digital_Filter		= 'MP';	% set the acceleration digital filter to either minimum phase, 'MP', linear phase 'LP' or off 'OFF'
  sCFG.Gyro_Digital_Filter 					  = 'MP';	    % set the gyro digital filter to either minimum phase, 'MP', linear phase 'LP' or off 'OFF'
  sCFG.Magnetometer_Digital_Filter 	  = 'MP';	    % set the magnetometer digital filter to either minimum phase, 'MP', linear phase 'LP' or off 'OFF'
  sCFG.Acceleration_Range 						= '2g';	    % set the range of the accelerometers to either '2g' for +/-2g or '6g' for +/-6g
  sCFG.Gyro_Range 										= '300deg';	% set the gyro range to either '300deg' for +/-300deg/s or '900deg' for +/-900 deg/s
  sCFG.Acceleration_Calibration			  = 'FACTORY';    % set which calibration coefficients to use, 'FACTORY' for the factory set, 'USER' for the user set or 'OFF' to disable calibration
  sCFG.Gyro_Calibration							  = 'FACTORY';    % set which calibration coefficients to use, 'FACTORY' for the factory set, 'USER' for the user set or 'OFF' to disable calibration
  sCFG.Magnetometer_Calibration			  = 'FACTORY';    % set which calibration coefficients to use, 'FACTORY' for the factory set, 'USER' for the user set or 'OFF' to disable calibration
  sCFG.Accelerometer_Cross_Axis				= 'ON' ; % set to 'ON' to enable accelerometer cross axis error compensation				
  sCFG.Gyro_Cross_Axis	        			= 'ON' ; % set to 'ON' to enable gyro cross axis error compensation
  sCFG.Magnetometer_Cross_Axis				= 'OFF' ; % set to 'ON' to enable magnetometer cross axis error compensation
  sCFG.Gyro_Specific_Force					  = 'ON' ; % set to 'ON' to enable gyro specific force error compensation        			
  sCFG.RPH_Algorithm									= 'Fusion';% set the attitude calculation algorithm to 'Legacy' or 'Fusion' where the latter uses the gyros in addition to accelerometer and magnetometer
  sCFG.RPH_Output_Format							= 'Quaternion';% set the data format to 'Quaternion' or 'Euler'
  sCFG.RPH_Azimuth_Reference					= 'Magnetometer';% set the azimuth reference to 'Magnetometer' or 'Disabled' where the latter provides no feedback for the azimuth angle, it is just the integrated gyro rate.
  sCFG.RPH_Azimuth_Reset							= 'Enabled' ;% set to 'Enabled' to allow the use of the navigation switch to reset the azimuth angle to zero when the magnetometer is not used for feedback
end



% ***********************************************************************************************
% Do not edit beyond this point!
% ***********************************************************************************************

CA_SF_Compensation = 0;
switch sCFG.Accelerometer_Cross_Axis
case 'ON'
CA_SF_Compensation = CA_SF_Compensation + 1;
end

switch sCFG.Gyro_Cross_Axis
case 'ON'
CA_SF_Compensation = CA_SF_Compensation + 2;
end

switch sCFG.Magnetometer_Cross_Axis
case 'ON'
CA_SF_Compensation = CA_SF_Compensation + 4;
end

switch sCFG.Gyro_Specific_Force
case 'ON'
CA_SF_Compensation = CA_SF_Compensation + 8;
end

CA_SF_property_string = num2str( CA_SF_Compensation , '%02X');


RPH_properties = 1;
switch sCFG.RPH_Algorithm
  case 'Fusion'
    RPH_properties = RPH_properties + 2;
end

switch sCFG.RPH_Output_Format
  case 'Quaternion'
    RPH_properties = RPH_properties + 4;
end

switch sCFG.RPH_Azimuth_Reference
  case 'Magnetometer'
    RPH_properties = RPH_properties + 0;
  otherwise
    RPH_properties = RPH_properties + 8;
end

switch sCFG.RPH_Azimuth_Reset
  case 'Enabled'
    RPH_properties = RPH_properties + 16;
end

RPH_property_string = num2str( RPH_properties , '%02X');


% this variable sets vibration, temperature and analog inputs always on, as there is little to be gained by turning them off
power_config_base = 8 + 32 + 64;

switch sCFG.Acceleration_Output_Data_Rate
  case 0
    acc_power_ctrl = 0;
  otherwise
    acc_power_ctrl = 2;
end

switch sCFG.Gyro_Output_Data_Rate
  case 0
    gyro_power_ctrl = 0;
  otherwise
    gyro_power_ctrl = 4;
end

switch sCFG.Magnetometer_Output_Data_Rate
  case 0
    mag_power_ctrl = 0;
  otherwise
    mag_power_ctrl = 16;
end

switch sCFG.Quaternion_Output_Data_Rate
  case 0
  otherwise
    mag_power_ctrl = 16;
    gyro_power_ctrl = 4;
    acc_power_ctrl = 2;
end

switch sCFG.Navigation_Switch_Output
  case true
    navsw_power_ctrl = 128;
  otherwise
    navsw_power_ctrl = 0;
end


pwr_ctrl_register = num2str( power_config_base + acc_power_ctrl + gyro_power_ctrl + mag_power_ctrl + navsw_power_ctrl, '%02X');


acc_ODR = num2str(round(sCFG.Acceleration_Output_Data_Rate) , '%02X');
gyro_ODR = num2str(round(sCFG.Gyro_Output_Data_Rate) , '%02X');
mag_ODR = num2str(round(sCFG.Magnetometer_Output_Data_Rate) , '%02X');
quaternion_ODR = num2str(round(sCFG.Quaternion_Output_Data_Rate) , '%02X');

switch sCFG.Acceleration_Digital_Filter
  case 'MP'
    acc_DF = num2str(round( 3 ) , '%02X');
  case 'LP'
    acc_DF = num2str(round( 1 ) , '%02X');
  case 'OFF'
    acc_DF = num2str(round( 0 ) , '%02X');
  otherwise
    acc_DF = num2str(round( 0 ) , '%02X');
end


switch sCFG.Gyro_Digital_Filter
  case 'MP'
    gyro_DF = num2str(round( 3 ) , '%02X');
  case 'LP'
    gyro_DF = num2str(round( 1 ) , '%02X');
  case 'OFF'
    gyro_DF = num2str(round( 0 ) , '%02X');
  otherwise
    gyro_DF = num2str(round( 0 ) , '%02X');
end


switch sCFG.Magnetometer_Digital_Filter
  case 'MP'
    mag_DF = num2str(round( 3 ) , '%02X');
  case 'LP'
    mag_DF = num2str(round( 1 ) , '%02X');
  case 'OFF'
    mag_DF = num2str(round( 0 ) , '%02X');
  otherwise
    mag_DF = num2str(round( 0 ) , '%02X');
end

switch sCFG.Acceleration_Range
  case '2g'
    acc_range = 0;
  case '6g'
    acc_range = 1;
  otherwise
    acc_range = 0;
end

switch sCFG.Gyro_Range
  case '300deg'
    scfg.gyro_range = 0;
  case '900deg'
    scfg.gyro_range = 4;
  otherwise
    scfg.gyro_range = 0;
end

switch sCFG.Gyro_Temperature_Output
  case true
    gyro_temperature_streaming = 16;
  otherwise
    gyro_temperature_streaming = 0;
end

mems_flags = num2str( acc_range + scfg.gyro_range + gyro_temperature_streaming ,'%02X');

switch sCFG.Acceleration_Calibration
  case 'FACTORY'
    acc_cal_bypass = 0;
    acc_cal_source = 0;
  case 'USER'
    acc_cal_bypass = 0;
    acc_cal_source = 1;
  otherwise % includes 'OFF'
    acc_cal_bypass = 1;
    acc_cal_source = 0;
end


switch sCFG.Gyro_Calibration
  case 'FACTORY'
    gyro_cal_bypass = 0;
    gyro_cal_source = 0;
  case 'USER'
    gyro_cal_bypass = 0;
    gyro_cal_source = 2;
  otherwise % includes 'OFF'
    gyro_cal_bypass = 2;
    gyro_cal_source = 0;
end


switch sCFG.Magnetometer_Calibration
  case 'FACTORY'
    mag_cal_bypass = 0;
    mag_cal_source = 0;
  case 'USER'
    mag_cal_bypass = 0;
    mag_cal_source = 4;
  otherwise % includes 'OFF'
    mag_cal_bypass = 4;
    mag_cal_source = 0;
end


cal_bypass_register = num2str( acc_cal_bypass + gyro_cal_bypass + mag_cal_bypass , '%02X');
cal_source_register = num2str( acc_cal_source + gyro_cal_source + mag_cal_source , '%02X');

% acc_ODR
% gyro_ODR
% mag_ODR

sk6config  = {...
  'PERIOD' , 1/sCFG.Receive_Parse_Rate ;...
  'Hardware input buffer size' , sCFG.Hardware_Input_Buff_Size ;...  
	'0002' , '06' ;... % set output data format to raw with 8 bit packet counter field, and ACK packets to raw also 
	'0000' , pwr_ctrl_register ;... % disable the navigation switch, power up mag, accel and gyro                              
	'0008' , acc_ODR ;... %set the accel, gyro and mag output data rates                                            
	'0009' , gyro_ODR ;... %set the accel, gyro and mag output data rates                                           
	'000A' , mag_ODR ;... %set the accel, gyro and mag output data rates                                            
	                         
  };
% sk6config  = {...
%   'PERIOD' , 1/sCFG.Receive_Parse_Rate ;...
%   'Hardware input buffer size' , sCFG.Hardware_Input_Buff_Size ;...  
% 	'0002' , '16' ;... % set output data format to raw with 8 bit packet counter field, and ACK packets to raw also 
% 	'0000' , pwr_ctrl_register ;... % disable the navigation switch, power up mag, accel and gyro                   
% 	'0004' , mems_flags ;... % set acc range and gyro range                                                         
% 	'0014' , acc_DF  ;... % set acceleration digital filter properties                                              
% 	'0015' , gyro_DF ;... % set gyro digital filter properties                                                      
% 	'0016' , mag_DF  ;... % set magnetometer digital filter properties                                              
% 	'0005' , cal_source_register ;... % use FACTORY or USER calibration sets?                                       
% 	'0006' , cal_bypass_register ;... % set the accel, gyro and mag calibration on or off                           
% 	'0046' , RPH_property_string ; ... % set the properties of the RPH packets                                      
% 	'0007' , CA_SF_property_string ;... % set the cross axis and specific force compensation                        
% 	'0017' , '01' ; ... % Quat legacy mode filter #1 enabled                                                        
% 	'0008' , acc_ODR ;... %set the accel, gyro and mag output data rates                                            
% 	'0009' , gyro_ODR ;... %set the accel, gyro and mag output data rates                                           
% 	'000A' , mag_ODR ;... %set the accel, gyro and mag output data rates                                            
% 	'000B' , quaternion_ODR ; ... % set the orientation output data rate                                            
%   };
  
%  '0002' , '16' ;... % set output data format to raw with 8 bit packet counter field, and ACK packets to raw also
%  '0000' , '7F' ;... % disable the navigation switch, power up mag, accel and gyro
%  '0004' , '05' ;... % set acc range and gyro range
%  '0014' , '03'  ;... % set acceleration digital filter properties
%  '0015' , '03' ;... % set gyro digital filter properties
%  '0016' , '03'  ;... % set magnetometer digital filter properties
%  '0005' , '00' ;... % use FACTORY or USER calibration sets?
%  '0006' , '00' ;... % set the accel, gyro and mag calibration on or off
%  '0046' , '00' ; ... % set the properties of the RPH packets
%  '0007' , '07' ;... % set the cross axis and specific force compensation
%  '0017' , '01' ; ... % Quat legacy mode filter #1 enabled
%  '0008' , '66' ;... %set the accel, gyro and mag output data rates
%  '000A' , '66' ;... %set the accel, gyro and mag output data rates
%  '0009' , '66' ;... %set the accel, gyro and mag output data rates
%  '000B' , '00'; ... % set the orientation output data rate
  

%   '0002' , '16' ;... % set output data format to raw with 8 bit packet counter field, and ACK packets to raw also
%   '0000' , pwr_ctrl_register ;... % disable the navigation switch, power up mag, accel and gyro
%   '0004' , mems_flags ;... % set acc range and gyro range
%   '0014' , acc_DF  ;... % set acceleration digital filter properties
%   '0015' , gyro_DF ;... % set gyro digital filter properties
%   '0016' , mag_DF  ;... % set magnetometer digital filter properties
%   '0005' , cal_source_register ;... % use FACTORY or USER calibration sets?
%   '0006' , cal_bypass_register ;... % set the accel, gyro and mag calibration on or off
%   '0046' , RPH_property_string ; ... % set the properties of the RPH packets
%   '0007' , CA_SF_property_string ;... % set the cross axis and specific force compensation
%   '0017' , '01' ; ... % Quat legacy mode filter #1 enabled
%   '0008' , acc_ODR ;... %set the accel, gyro and mag output data rates
%   '000A' , gyro_ODR ;... %set the accel, gyro and mag output data rates
%   '0009' , mag_ODR ;... %set the accel, gyro and mag output data rates
%   '000B' , quaternion_ODR ; ... % set the orientation output data rate

end
